#include "../_includes.h"

// //侧面网放预装；怼墙校准；挖角落粽子；推通道粽子
void auto_left_2() {
  gyro_reset();
  turn_pid_adjust_agl_absolute(-90,100,1000,Block::yes);
  // catapult.set_speed(100);
  // waitUntil(catapult.get_enc() >= 200);
  // catapult.stop();

  // chassis_r.reset_enc();
  // chassis_l.hold_stop();
  // while (chassis_r.get_enc() <= 3400) {
  //   chassis_r.set_speed(30);
  // }
  // chassis_r.stop();

  // wing_lock.set(true);
  // chassis.stable_forward_dist_relative(300, 100, 800, Block::yes);
  // gyro_reset();
  // wait(200);
  // turn_pid_adjust_agl_absolute(-225, 80, 1000, Block::yes);
  // wing_lock.set(false);
  // chassis.stable_forward_dist_relative(-400, 100, 800);
  // turn_pid_adjust_agl_absolute(-243, 80, 1000, Block::yes);
  // chassis.stable_forward_dist_relative(-780, 100, 800);
}
// void auto_left_2() {
//   dig.act_for_enc_absolute(enc_dig_upright, 100, 1000, Block::no); //解锁
//   chassis.stable_forward_dist_relative(320, 80, 1000, Block::yes);
//   turn_pid_adjust_agl_absolute(20, 100, 1000, Block::yes); //左转向
//   intake.set_voltage(-100);
//   wait(300);
//   chassis.reset_enc();
//   chassis.FT_set_voltage(100, 0);
//   timer t;
//   while (t.time() < 1200) {
//     if (chassis.get_dist_mm() > 300)
//       break;
//     if (chassis.get_dist_mm() > 200)
//       intake.set_voltage(15);
//     wait(5);
//   }

//   repeat(1) {
//     wait(400);
//     chassis.fast_forward_dist_relative(-200, 80, 800, Block::yes);
//     wait(300);
//     chassis.fast_forward_dist_relative(250, 100, 1300, Block::yes);
//   }

//   chassis.FT_set_voltage(-20, 0); //抵侧网杆子
//   wait(500);
//   gyro_reset(); //侧网为新的陀螺仪0点
//   chassis.stable_forward_dist_relative(-50, 100, 1000, Block::yes);
//   turn_pid_adjust_agl_absolute(90, 100, 800, Block::yes); //转向
//   chassis.stable_forward_dist_relative(-120, 100, 1000, Block::yes);
//   chassis.FT_set_voltage(-20, 0); //抵墙
//   wait(500);
//   gyro_reset(); //墙作为新的陀螺仪0点
//   chassis.stable_forward_dist_relative(100, 100, 1000, Block::yes);
//   turn_pid_adjust_agl_absolute(45, 100, 900, Block::yes); //左转向
//   chassis.stable_forward_dist_relative(320, 100, 1000, Block::yes);
//   turn_pid_adjust_agl_absolute(135, 100, 1000, Block::yes); //左转向
//   dig.act_for_enc_absolute(enc_dig_prepare_to_cellect_triball, 100, 1000,
//                            Block::no); // dig提前就位
//   intake.set_voltage(100);
//   chassis.stable_forward_dist_relative(280, 100, 1000, Block::yes);
//   chassis.FT_set_voltage(5, 0); //抵住杆子
//   dig.act_for_enc_absolute(enc_dig_collect_triball, 100, 1000,
//                            Block::yes); //挖粽子
//   dig.stop(hold);                       //稳住

//   chassis.stable_forward_dist_relative(-250, 100, 1500, Block::yes); //后退
//   chassis.stable_forward_dist_relative(50, 100, 1500, Block::yes);
//   wait(500);
//   dig.act_for_enc_absolute(enc_dig_prepare_hit_hang_stick, 100, 1000,
//                            Block::no); //挖挖回位
//   wait(100);
//   turn_pid_adjust_agl_absolute(45, 100, 1000, Block::yes); //转向平行于三角杆
//   chassis.stable_forward_dist_relative(350, 100, 1500,
//                                        Block::yes); //前进一点到接近挂杆
//   intake.stop(hold);
//   turn_pid_adjust_agl_absolute(0, 100, 1000, Block::yes); //转向挂杆
//   toss.act_for_enc_absolute(95, 100, 1000,
//                             Block::no); //弹射辅助防止过线
//   wait(100);
//   intake.mtr_set_voltage(-100); //吐粽子
//   chassis.stable_forward_dist_relative(800, 100, 1500,
//                                        Block::yes); //前进，并碰杆
//   dig.set_voltage(-30);                             //保持压压杆子
//   chassis.stable_forward_dist_relative(-30, 100, 500,
//                                        Block::yes); //后退
//   cout << "auctual time:" << t_auto.time() << endl;
//   while_wait(t_auto.time() < 15000);
// }
